package fr.utbm.set.safeplatoon.common.merging.data;

import java.util.ArrayList;

import javax.swing.event.EventListenerList;
import javax.vecmath.Point2d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector2d;

 




import fr.utbm.set.safeplatoon.common.merging.data.listner.LmsListener;
import fr.utbm.set.safeplatoon.common.merging.network.listner.EntityListener;
import fr.utbm.set.safeplatoon.common.merging.sma.environment.WorldModel;
import fr.utbm.set.safeplatoon.common.merging.sma.environment.objet.StandardEntity;
import fr.utbm.set.safeplatoon.common.merging.util.GeometryUtil;



/** Cette classe decrit un telemetres laser
 * 
 * auteur B dafflon pour le laboratoire Systemes et Transport UTBM
 */

public class Lms implements LmsListener {

	private int id;
	private Point3d postition = new Point3d(0.0,0.0,0.0);
	private Point3d orientation = new Point3d(0,0,0);
	private float angle;
	private double step;
	private float distanceMax;
	private String ip;
	private int port;

	private ArrayList<StandardEntity> data = new ArrayList<StandardEntity>();
	private ArrayList<StandardEntity> limit = new ArrayList<StandardEntity>();
	private ArrayList< ArrayList<StandardEntity>> limitTemp = new ArrayList< ArrayList<StandardEntity>>();


	private final EventListenerList listeners = new EventListenerList();


	@SuppressWarnings("rawtypes")
	public Lms( double step, int angle, int distMax, WorldModel s){
		this.setStep(step);
		this.setAngle(angle);
		this.setDistanceMax(distMax);
		 
		s=s;
 
		for(double i1=  (-1*this.getDistanceMax()-5); i1<this.getDistanceMax()+5;i1=(i1+0.2)){
			Point2d positionMesh = new Point2d(i1, -0.5);

			StandardEntity entity = new StandardEntity(positionMesh,s);
			s.addObject(entity);
		}
	}

	 
	public int getId() {
		return id;
	}

	public void setId(int id) {
		this.id = id;
	}

	public ArrayList<ArrayList<StandardEntity>> getLimitTemp() {
		return limitTemp;
	}


	public void setLimitTemp(ArrayList<ArrayList<StandardEntity>> limitTemp) {
		this.limitTemp = limitTemp;
	}

	public Point3d getPostition() {
		return postition;
	}


	public void setPostition(Point3d postition) {
		this.postition = postition;
	}

	public Point3d getOrientation() {
		return orientation;
	}

	public void setOrientation(Point3d orientation) {
		this.orientation = orientation;
	}

	public float getAngle() {
		return angle;
	}

	public void setAngle(float angle) {
		this.angle = angle;
	}

	public double getStep() {
		return step;
	}

	public void setStep(double step2) {
		this.step = step2;
	}


	public void setIp(String ip) {
		this.ip = ip;
	}


	public String getIp() {
		return ip;
	}


	public void setPort(int port) {
		this.port = port;
	}


	public int getPort() {
		return port;
	}


	 



	public Point2d getPosition(int index){
		return new Point2d();
	}




	public float getDistanceMax() {
		return distanceMax;
	}

	//mise a jour des obstacles
	public void setData(ArrayList<Double> dataInteger) {
		
		//System.out.println("data "+dataInteger.size());
		Vector2d vecDir = new Vector2d(0,1);
		double angle = Math.toRadians(this.getStep());

		GeometryUtil.turnVector(vecDir, Math.toRadians(this.getOrientation().y) + ((this.angle)/this.step/2)*angle);


		for (int i1=0; i1< dataInteger.size();i1=i1+1) {
			//vecDir.add(new Vector2d(this.getPostition().x,this.getPostition().z));
			boolean notNull=dataInteger.get(i1)>50;
			if(notNull){
				vecDir.scale(dataInteger.get(i1)/1000.0);
			}
		 
			Point2d pts = new Point2d(this.getPostition().x+vecDir.x, this.getPostition().z+vecDir.y);
			//System.out.println(vecDir);
			//System.out.println(dataInteger.get(i1));
			
			if(this.data.get(i1).getLocation()!=pts && notNull && i1<180*2)
			{
				Vector2d vecDirCpt = new Vector2d(vecDir);

				//mise a jour du point d'impacte du lms
				this.data.get(i1).setLocation(new Point2d(pts.x,pts.y));

				//raz de la bordure
				for(int i2=0;i2<this.getLimitTemp().get(i1).size();i2++){
					vecDirCpt.normalize();
					vecDirCpt.scale(this.distanceMax+5);


					this.getLimitTemp().get(i1).get(i2).setLocation( new Point2d(this.getPostition().x+vecDirCpt.x, this.getPostition().z+vecDirCpt.y));

				}

				//Point2d pts2 = new Point2d(this.data.get(i1-1).getPosition2D().x,this.data.get(i1-1).getPosition2D().y);

				//mise a jour de la bordure
				vecDirCpt = new Vector2d(vecDir);
				float start = (float) vecDirCpt.length();
				for(int i2=0;i2<this.getLimitTemp().get(i1).size();i2++){
					vecDirCpt.normalize();
					vecDirCpt.scale(start+(float)i2/2);
					if(vecDirCpt.length()<this.distanceMax){
						this.getLimitTemp().get(i1).get(i2).setLocation( new Point2d(this.getPostition().x+vecDirCpt.x, this.getPostition().z+vecDirCpt.y));
						//System.out.println(" on ferme  sur" +i1 +" "+ i2);
					}
				}
			}

			vecDir.normalize();
			GeometryUtil.turnVector(vecDir, -angle);

		}

		//mise a jour des entités

		//System.out.println("MAJ FIN "+(System.thisTimeMillis()-debut));

	}



	//mise a jour des obstacles
	public void setData(ArrayList<Double> dataInteger, ArrayList<Double> ideoData) {

		Vector2d vecDir = new Vector2d(0,1);
		double angle = Math.toRadians(this.getStep());

		GeometryUtil.turnVector(vecDir,   Math.toRadians(this.getOrientation().y) + ((this.angle-2)/2)*angle);

		//System.out.println("------------------------------------");
		for (int i1=0; i1< dataInteger.size();i1++) {
			//vecDir.add(new Vector2d(this.getPostition().x,this.getPostition().z));


			vecDir.scale(dataInteger.get(i1));
			//vecDir.scale(dataInteger.get(dataInteger.size()-1-i1));
			Point2d pts = new Point2d(this.getPostition().x+vecDir.x, this.getPostition().z+vecDir.y);

		 

 

			if(this.data.get(i1).getLocation()!=pts)
			{
				Vector2d vecDirCpt = new Vector2d(vecDir);

				//mise a jour du point d'impacte du lms
				this.data.get(i1).setLocation(new Point2d(pts.x,pts.y));

				//raz de la bordure
				for(int i2=0;i2<this.getLimitTemp().get(i1).size();i2++){
					vecDirCpt.normalize();
					vecDirCpt.scale(this.distanceMax+5);


					this.getLimitTemp().get(i1).get(i2).setLocation( new Point2d(this.getPostition().x+vecDirCpt.x, this.getPostition().z+vecDirCpt.y));

				}

				//Point2d pts2 = new Point2d(this.data.get(i1-1).getPosition2D().x,this.data.get(i1-1).getPosition2D().y);

				//mise a jour de la bordure
				vecDirCpt = new Vector2d(vecDir);
				float start = (float) vecDirCpt.length();
				for(int i2=0;i2<this.getLimitTemp().get(i1).size();i2++){
					vecDirCpt.normalize();
					vecDirCpt.scale(start+(float)i2/2);
					if(vecDirCpt.length()<this.distanceMax){
						this.getLimitTemp().get(i1).get(i2).setLocation( new Point2d(this.getPostition().x+vecDirCpt.x, this.getPostition().z+vecDirCpt.y));
						//System.out.println(" on ferme  sur" +i1 +" "+ i2);
					}
				}
			}

			vecDir.normalize();
			GeometryUtil.turnVector(vecDir, -angle);

		}


		//mise a jour des entités

		//System.out.println("MAJ FIN "+(System.thisTimeMillis()-debut));

	}

	public void setArrayData(ArrayList<StandardEntity> data) {

		this.data = data;
	}

	public ArrayList<StandardEntity> getData() {
		return data;
	}


	public void setLimit(ArrayList<StandardEntity> limit) {
		this.limit = limit;
	}


	public ArrayList<StandardEntity> getLimit() {
		return limit;
	}

	public EntityListener[] getEntityListener() {
		return listeners.getListeners(EntityListener.class);
	}

	public void addEntityListener(EntityListener listener) {
		listeners.add(EntityListener.class, listener);
	}

	public void removeEntityListener(EntityListener listener) {
		listeners.remove(EntityListener.class, listener);
	}


	public void setDistanceMax(int parseInt) {
		// TODO Auto-generated method stub
		this.distanceMax = parseInt;
	}


}
